#include <16F819.h>
#include <stdlib.h>



#FUSES NOWDT                    //No Watch Dog Timer
#FUSES INTRC_IO                 //Internal RC Osc, no CLKOUT
#FUSES PUT                      //Power Up Timer
#FUSES NOMCLR                   //Master Clear pin used for I/O
#FUSES BROWNOUT                 //Reset when brownout detected
#FUSES NOLVP                    //No low voltage prgming, B3(PIC16) or B5(PIC18) used for I/O
#FUSES NOCPD                    //No EE protection
#FUSES NOWRT                    //Program memory not write protected
#FUSES NODEBUG                  //No Debug mode for ICD
#FUSES NOPROTECT                //Code not protected from reading

#use delay(Internal=8M)

//------------ Pines del LCD ---------------------//
#define LCD_E     PIN_A0
#define LCD_CK    PIN_A1   
#define LCD_DAT   PIN_A2
//--------------------------------------------------//

#include "flex_lcdSlave.c"
//Variables globales-------------------------------------

unsigned int data=2;
#define SERVO1 PIN_B7
#define SERVO2 PIN_B6
#define SERVO3 PIN_B3
#define SERVO4 PIN_B0
#define SERVO5 PIN_A2
#define SERVO6 PIN_A5
#define SERVO7 PIN_A6
#define SERVO8 PIN_A7

//valores aproximados los calcule con simulador
//falta medir todos con osciloscopio.

const int16 Ticks4Window = 4999; // PWM Window for servo = 2.5 ms x 8 = 20 ms
const int16 Ticks4Minimum = 1399; // PWM High for Minimum Position = 0.7 ms
const int16 Ticks4Center = 2999; // PWM High for Center Position = 1.5 ms
const int16 Ticks4Maximum = 4599; // PWM High for Maximum Position = 2.3 ms




//RAM


int1 flagcommand; //bandera para saber si presionamos enter
//Debe de llevar todo el arreglo un valor asignado, ya que si no es así
//el timer1 se cargará de forma inmediata con el valor 65535, causando
//un desborde de 200ns lo cual impedirá que trabaje de forma adecuada 
//la temporización.
static int16 Servo_PWM[8]={Ticks4Minimum,Ticks4Center,Ticks4Center,Ticks4Center,Ticks4Center,Ticks4Center,Ticks4Center,Ticks4Center};
static int8 Servo_Idx=0;
static int1 SERVO1_ON=1;
static int1 SERVO2_ON=0;
static int1 SERVO3_ON=0;
static int1 SERVO4_ON=0;
static int1 SERVO5_ON=0;
static int1 SERVO6_ON=0;
static int1 SERVO7_ON=0;
static int1 SERVO8_ON=0;
static int1 flag_Phase;
static int16 Ticks4NextInterrupt=53035;
int16 posI;

//Interrupciones-----------------------
void command_process(int posicion){

/*
El formato de los comandos es el siguiente:
[a...e] [123] [xx]
servo posicion velocidad
primer byte un caracter alfabético de entre a y e que define al servo
tres bytes para definir la posicion del servo
dos bytes para definir la velocidad de movimiento del servo
a12301 {enter}
mueve el servo 1 a 123 grados de la posicion de origen a la velocidad 01
*/
   int16 i;
   int1 DerIzq = 1; //bandera para saber si va a 180 o a 0 grados
   // DerIzq = 1   Indicará que vamos de 0 a 180 por tanto suma
   // DerIzq = 0   Indicará que vamos de 180 a 0 por tanto resta

   char servo[2];


   int8 vel;
   
   int16 posF;
   int16 vueltas=1;
   
   flagcommand=0; //Es desactivada la flag cuando el comando esta pendiente
   printf(lcd_putc,"procesando ...\r\n");
   //Esta condicional sirve para asignar el valor del valor inicial
   //ke en diferencia con el valor final moveran al servo en 
   //uno o en otro sentido.
            posI = servo_PWM[0];
            break;

   
   //Determinamos el numero de vueltas
/*   3400   -- 0000 --- 0.03
   3398
const int16 Ticks4Window = 5500; // PWM Window for servo = 2.5 ms x 8 = 20 ms
const int16 Ticks4Minimum = 1399; // PWM High for Minimum Position = 0.7 ms
const int16 Ticks4Center = 3350; // PWM High for Center Position = 1.5 ms
const int16 Ticks4Maximum = 4599; // PWM High for Maximum Position = 2.3 ms
   si 3350 corresponde a 90° entonces 180° es a 6700  en teoria 
   un grado corresponde a un incremento de 37
   Dado que cada incremento de 37 nos da un adelanto de 1 entonces tenemos que
   el maximo es 12259
   */
   posF=17.77*posicion+1399;
   
   if(posF>posI){
    vueltas = posF - posI;
    vueltas = vueltas / vel;
    DerIzq = 1;
   }
   else
   {
    vueltas = posI - posF;
    vueltas = vueltas/ vel;
    DerIzq = 0;
   }
   

   printf(lcd_putc"posI:%lu posF:%lu\r\n",posI,posF);
   printf(lcd_putc,"vueltas: %lu \r\n",vueltas);

/*
   Debe de leerse el valor que tiene del servo desde el array
   ´para determinar el valor inicial y no unicamente dejarlo
   como valor final. esto causa el eeror de que al mover
   multiples motores con una unica variable se moveran en conse-
   cuencia del anterior
*/
  // posI = posF;  Esto no es correcto
   
   if(DerIzq)
   {
         switch(servo[0])
         {
         case 'a':  //mueve al servo uno en incrementos de vel hasta llegar a vueltas
            for(i=0;i<vueltas;i++){
            Servo_PWM[0]+=vel;
            delay_ms(1);}
            break;
         default:
            printf(lcd_putc"X_x\r\n");
         }
   }
   else{
         switch(servo[0])
         {
            case 'a':  //mueve al servo uno en incrementos de vel hasta llegar a vueltas
                  for(i=0;i<vueltas;i++){
                  Servo_PWM[0]-=vel;
                  delay_ms(1);}
                  break;
            default:
                  printf(lcd_putc,"X_x\r\n");
          }   
   }
   
   
//   initBuff();  //inicializamos el buffer
   printf(lcd_putc,"Procesado\r\n");
   }
   

#int_TIMER1
void  TIMER1_isr(void) 
{
	if(flag_Phase==0){
    if(Servo_Idx==0 && SERVO1_ON) output_high(SERVO1);
    if(Servo_Idx==1 && SERVO2_ON) output_high(SERVO2);
    if(Servo_Idx==2 && SERVO3_ON) output_high(SERVO3);
    if(Servo_Idx==3 && SERVO4_ON) output_high(SERVO4);
    if(Servo_Idx==4 && SERVO5_ON) output_high(SERVO5);
    if(Servo_Idx==5 && SERVO6_ON) output_high(SERVO6);
    if(Servo_Idx==6 && SERVO7_ON) output_high(SERVO7);
    if(Servo_Idx==7 && SERVO8_ON) output_high(SERVO8);
    Ticks4NextInterrupt = 65535 - Servo_PWM[Servo_Idx];
    set_timer1(Ticks4NextInterrupt);
  }
  if(flag_Phase==1){
    if(Servo_Idx==0 && SERVO1_ON) output_low(SERVO1);
    if(Servo_Idx==1 && SERVO2_ON) output_low(SERVO2);
    if(Servo_Idx==2 && SERVO3_ON) output_low(SERVO3);
    if(Servo_Idx==3 && SERVO4_ON) output_low(SERVO4);
    if(Servo_Idx==4 && SERVO5_ON) output_low(SERVO5);
    if(Servo_Idx==5 && SERVO6_ON) output_low(SERVO6);
    if(Servo_Idx==6 && SERVO7_ON) output_low(SERVO7);
    if(Servo_Idx==7 && SERVO8_ON) output_low(SERVO8);
    Ticks4NextInterrupt = 65535 - Ticks4Window + Servo_PWM[Servo_Idx];
    set_timer1(Ticks4NextInterrupt);
    if(++Servo_Idx>7) Servo_Idx=0;  //tenia >7
  }
  ++flag_Phase;
}

//-------------------------------------
void main(void)
{	
	setup_oscillator(OSC_8MHZ);
	setup_adc_ports(NO_ANALOGS);
	setup_adc(ADC_OFF);
	setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
	setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);
	setup_timer_2(T2_DISABLED,0,1);
	setup_spi(SPI_SLAVE | SPI_L_TO_H | SPI_CLK_DIV_16);
	//activando terminales
	set_tris_a(0x08);  //0011 0010
	set_tris_b(0x32);  //0000 1000
	
	lcd_init();
	lcd_setcursor_vb(1,1);
	printf(lcd_putc,"Reloj activado");
	output_low(PIN_A1); 
	output_low(PIN_A0);
	delay_ms(2000);
	printf(lcd_putc,"\n\r Servos abajo");
	output_low(SERVO1);
  	output_low(SERVO2);
 	output_low(SERVO3);
  	output_low(SERVO4);
 	output_low(SERVO5);
 	output_low(SERVO6);
 	output_low(SERVO7);
 	output_low(SERVO8);

	set_timer1(Ticks4NextInterrupt);
  	enable_interrupts(int_timer1);
  	enable_interrupts(global);
     	posI=Ticks4Center;

	while(1)
	{
		while(!spi_data_is_in() && !input(PIN_B5)){
		};
		if(spi_data_is_in() && input(PIN_B5))
		{
			
			data=spi_read();
			output_high(PIN_A0);
			command_process(data);
			delay_ms(1);
			output_low(PIN_A0);
		}
	}
}